-----------------------------------------------------------------------
-- File:        Turnout_Controller.ads
-- Description: Provide an interface to the hardware control of turnouts,
--				ensures the turning of requested turnout is done properly
--				and reports to enquiring modules about turnouts status
--
-- Requires:    Io_Ports,Unsigned_Types,Raildefs,Dio192defs, Dda06defs,
--              Int32defs
--
--
-- Author:     Maria Mitrevska
-----------------------------------------------------------------------
with Ada.Text_Io;  --*
with Io_Ports;
with Unsigned_Types;
use Unsigned_Types;  -- 1.8
with Raildefs,
   Dio192defs, Dda06defs, Int32defs, Halls2;
use Raildefs;

package body Turnout_Controller is --ACTIVE (PROTECTED and SPORADIC objects)

   --Declare types and variables
   type Turnout_Drive_Array is array (Raildefs.Turnout_Idx range 0 .. 2) of Dio192defs.Turnout_Drive_Register;
   type Turnout_State is array (Raildefs.Turnout_Id) of Boolean;
   type Turnout_Desired_Pos is array (Raildefs.Turnout_Id) of Raildefs.Turnout_Pos;

   --A counting down delay to ensure the Get_Status function only return the updated status of the turnout (not the one before turning initiated)
   Setting_Turnout_Delay :          array (Raildefs.Turnout_Id) of Natural := (others => 0);
   -- Number of turnouts
   N                     : constant                                        := 16;            --17?
   type Index_Type is mod N;
   type Item_Array is array (Index_Type) of Turnout_Id;

   task Worker ;

   protected Object is

      --Set turnout
      procedure Set_Turnout (
            Tn  : in     Raildefs.Turnout_Id;
            Dir : in     Raildefs.Turnout_Pos);

      --Init turnouts
      procedure Init_Turnouts;

      --Get turnout position
      function Get_Position (
            Tn : in     Raildefs.Turnout_Id)
        return Turnout_Pos;

      --Retry turnout
      procedure Retry_Turnout (
            Tn : in     Raildefs.Turnout_Id);


   private
      Tn_Drives : Turnout_Drive_Array;
      Tn_Position : Raildefs.Tn_State_Array:=(others => Straight);
      Tn_Desired_Position: Turnout_Desired_Pos;

   end Object;


   --PROTECTED
   protected body Object is

      --Set Turnout
      procedure Set_Turnout (
            Tn  : in     Raildefs.Turnout_Id;
            Dir : in     Raildefs.Turnout_Pos -- ignore if Middle
) is

         use Dio192defs;
         Ndx : Raildefs.Turnout_Idx := (Tn - 1) / 8;
         Bit : Raildefs.Turnout_Idx := (Tn - 1)
         mod 8;
         Drive : Dio192defs.Turnout_Drive_Bit;
      begin
         Tn_Desired_Position(Tn) := Dir;
         case Dir is
            when Middle =>
               Tn_Position(Tn):=Middle;
               return;
            when Straight =>
               Drive := Straighten;
               Tn_Position(Tn):=Straight;
            when Turned =>
               Drive := Turn;
               Tn_Position(Tn):=Turned;
         end case;
         Tn_Drives(Ndx)(Bit) := Drive;
         Io_Ports.Write_Io_Port(Turnout_Drive_Addr(Ndx), Unsigned(
               Tn_Drives(
                  Ndx)) );
         Turnout_Controller.Set_Status(Tn,False);
         Setting_Turnout_Delay(Tn) := 1;
         Turnout_Controller.Start(Tn); -- start turnout status update
      end Set_Turnout;

      --Init Turnouts
      procedure Init_Turnouts is

         use Dio192defs;
         use Unsigned_Types;

         Temp : Unsigned_8;

      begin

         for J in Tn_Drives'range loop
            Tn_Drives(J) := Turnout_Drive_Init;
            Io_Ports.Write_Io_Port(Turnout_Drive_Addr(J), Unsigned(
                  Tn_Drives(J)));
         end loop;

      end Init_Turnouts;

      --Retry turnout
      procedure Retry_Turnout (
            Tn : in     Raildefs.Turnout_Id) is
      begin
         if(Get_Position(Tn) = Middle) then -- only when failed
            Set_Turnout(Tn,Tn_Desired_Position(Tn));
         end if;
         Ada.Text_Io.Put_Line("Retry Tn...."&Tn'Img&"Desired  "&Tn_Desired_Position(Tn)'Img); --*
      end Retry_Turnout;


      --Get turnout position
      function Get_Position (
            Tn : in     Raildefs.Turnout_Id)
        return Turnout_Pos is
         Position : Turnout_Pos;
      begin

         Position:=Tn_Position(Tn);
         return Position;

      end Get_Position;

   end Object;


   protected Turn is

      --Get turnout status
      function Get_Status (
            Tn : in     Raildefs.Turnout_Id)
        return Boolean;

      --Set turnout status
      procedure Set_Status (
            Tn     : in     Raildefs.Turnout_Id;
            Status : in     Boolean);

      --Start turnout status update
      procedure Start (
            T_Id : in     Turnout_Id);

      --Wait start
      entry Wait_Start (
            T_Id     :    out Turnout_Id;
            Over_Run :    out Boolean);


   private

      Tn_Status : Turnout_State :=(others => True);
      Items: Item_Array;
      Iin, Jout : Index_Type := 0;
      Count : Integer := 0;
      Item_Available,
         Too_Fast : Boolean := False;

   end Turn;


   --PROTECTED object
   protected body Turn is


      --Get turnout status
      function Get_Status (
            Tn : in     Raildefs.Turnout_Id)
        return Boolean is
         Status : Boolean;
      begin
         if(Setting_Turnout_Delay(Tn) /= 0) then
            return False;
         end if;
         Status:=Tn_Status(Tn);
         return Status;

      end Get_Status;

      --Set turnout status
      procedure Set_Status (
            Tn     : in     Raildefs.Turnout_Id;
            Status : in     Boolean) is
      begin
         Tn_Status(Tn):=Status ;
      end Set_Status;

      --Start turnout status update
      procedure Start (
            T_Id : in     Turnout_Id) is
      begin
         if( Count >= N) then
            Too_Fast := True;
         end if;

         -- ignore old values
         Items (Iin) := T_Id;
         Iin := Iin + 1;
         Count := Count + 1;
         Item_Available := True;
      end Start;

      --Returns the item in the buffer
      entry Wait_Start (
            T_Id     :    out Turnout_Id;
            Over_Run :    out Boolean) when Item_Available is
      begin
         T_Id:= Items(Jout);
         Jout := Jout + 1;
         Count := Count - 1;
         Over_Run := Too_Fast;
         if(Count = 0) then
            Item_Available := False;
         end if;
         Too_Fast := False;
      end Wait_Start;

   end Turn;


   --Public procedures

   --Set Turnout
   procedure Set_Turnout (
         Tn  : in     Raildefs.Turnout_Id;
         Dir : in     Raildefs.Turnout_Pos -- ignore if Middle
) is

   begin

      Object.Set_Turnout(Tn,Dir);
   end Set_Turnout;


   -- Init Turnouts
   procedure Init_Turnouts is
      use Dio192defs;
      Temp : Unsigned_8;
   begin

      Object.Init_Turnouts;

   end Init_Turnouts;


   --Get status of turnout
   function Get_Status (
         Tn : in     Raildefs.Turnout_Id)
     return Boolean is
      Status : Boolean;

   begin

      Status:=Turn. Get_Status (Tn);
      return Status;

   end Get_Status;


   --Set status of turnout
   procedure Set_Status (
         Tn     : in     Raildefs.Turnout_Id;
         Status : in     Boolean) is
   begin
      Turn.Set_Status (Tn,Status);
   end Set_Status;

   --Retry turnout
   procedure Retry_Turnout (
         Tn : in     Raildefs.Turnout_Id) is
   begin
      Object.Retry_Turnout(Tn);
   end Retry_Turnout;


   --Get turnout position
   function Get_Position (
         Tn : in     Raildefs.Turnout_Id)
     return Turnout_Pos is
   begin
      return Object.Get_Position (Tn);
   end Get_Position;

   --Start turnout status update
   procedure Start (
         T_Id : in     Turnout_Id) is
   begin
      Turn.Start(T_Id);

   end Start;

   --Worker thread
   task body Worker is
      Req  : Turnout_Id;
      Oops : Boolean    := False;

      procedure Wait (
            T_Id : in     Turnout_Id) is

         use Dio192defs;

         --Tn_Status : Turnout_Status_Array := (others => Dio192defs.All_In_Position);

         Ndx : Raildefs.Turnout_Idx := (T_Id - 1) / 8;
         Bit : Raildefs.Turnout_Idx := (T_Id - 1)
         mod 8;

         Status : Dio192defs.Turnout_Status_Bit;

         Value,
         Temp,
         Temp2 : Unsigned_8;

      begin
         Temp2:=2#00000001#;
         delay 0.2;
		 -- Ensuring the status is false before turnout has time to complete
         Setting_Turnout_Delay(T_Id) := 0;
         loop
            Io_Ports.Read_Io_Port(Turnout_State_Addr(Ndx), Temp);
            Value:=Shift_Right((Temp and Shift_Left(Temp2 ,Natural(Bit))),
               Natural(Bit));
            exit when Value= 2#00000001#;
         end loop;
      end Wait;

   begin
      loop
         Turn.Wait_Start(Req,Oops);
         if not Oops then
            Wait(Req);
            Turn.Set_Status(Req,True);
         end if;
      end loop;
   end Worker;


end Turnout_Controller;